In [1]:
%%HTML
<style> code {background-color : pink !important;} </style>

Advanced Lane Finding Project

The goals / steps of this project are the following:

  • Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
  • Apply a distortion correction to raw images.
  • Use color transforms, gradients, etc., to create a thresholded binary image.
  • Apply a perspective transform to rectify binary image ("birds-eye view").
  • Detect lane pixels and fit to find the lane boundary.
  • Determine the curvature of the lane and vehicle position with respect to center.
  • Warp the detected lane boundaries back onto the original image.
  • Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.
In [2]:
### ALL NECESSARY IMPORTS ###
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import pickle
import matplotlib.image as mpimg

Step 1. Camera Calibration

In [3]:
%matplotlib qt5

# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1,2)

# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.

# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')

# Step through the list and search for chessboard corners
for idx, fname in enumerate(images):
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # Find the chessboard corners
    ret, corners = cv2.findChessboardCorners(gray, (9,6), None)

    # If found, add object points, image points
    if ret == True:
        objpoints.append(objp)
        imgpoints.append(corners)

        # Draw and display the corners
        cv2.drawChessboardCorners(img, (9,6), corners, ret)
        write_name = 'output_images/corners_found'+str(idx)+'.jpg'
        cv2.imwrite(write_name, img)
        cv2.imshow('img', img)
        cv2.waitKey(100)

cv2.destroyAllWindows()

And now we calibrate the Camera feeding the cv2 calibrateCamera function with the lists of distorted chessboard corners, and their objpoints.

In [4]:
%matplotlib inline

# Test undistortion on an image
img = cv2.imread('camera_cal/calibration1.jpg')
img_size = (img.shape[1], img.shape[0])

# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, img_size,None,None)

dst = cv2.undistort(img, mtx, dist, None, mtx)
cv2.imwrite('output_images/test_undist.jpg',dst)

# Save the camera calibration result for later use (we won't worry about rvecs / tvecs)
dist_pickle = {}
dist_pickle["mtx"] = mtx
dist_pickle["dist"] = dist
pickle.dump( dist_pickle, open( "camera_cal/calibration_pickle.p", "wb" ) )
#dst = cv2.cvtColor(dst, cv2.COLOR_BGR2RGB)
# Visualize undistortion
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(20,10))
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=30)
ax2.imshow(dst)
ax2.set_title('Undistorted Image', fontsize=30)
Out[4]:
<matplotlib.text.Text at 0x119c8fc18>

Step 2. Applying Distorsion Correction

In [5]:
# We load the distorsion pickle file data obtained in the previous section.
dist_pickle = pickle.load( open( "camera_cal/calibration_pickle.p", "rb"))
mtx = dist_pickle["mtx"]
dist = dist_pickle["dist"]

images = glob.glob('./test_images/test*.jpg')

for idx, fname in enumerate(images):
    img = cv2.imread(fname)
    img = cv2.undistort(img, mtx, dist, None, mtx)

    result = img
    
    write_name = './output_images/undist' + str(idx+1) + '.jpg'
    cv2.imwrite(write_name, result)

Step 3. Color & Gradient Thresholds --> Binary Image

First, we define the color and gradient threshold functions that we have seen during the lessons, so that later we can experiment with different combinations of them.

In [6]:
# Define a function that takes an image, gradient orientation,
# and threshold min / max values.
def abs_sobel_thresh(img, orient='x', thresh=(0, 255)):
    # Convert to grayscale
    
    #gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    r,g,b = cv2.split(img)
    gray = np.uint8(.5*r+.5*g)

    
    # Apply x or y gradient with the OpenCV Sobel() function
    # and take the absolute value
    if orient == 'x':
        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 1, 0))
    if orient == 'y':
        abs_sobel = np.absolute(cv2.Sobel(gray, cv2.CV_64F, 0, 1))
    # Rescale back to 8 bit integer
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # Create a copy and apply the threshold
    binary_output = np.zeros_like(scaled_sobel)
    # Here I'm using inclusive (>=, <=) thresholds, but exclusive is ok too
    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1

    # Return the result
    return binary_output

# Define a function to return the magnitude of the gradient
# for a given sobel kernel size and threshold values
def mag_thresh(img, sobel_kernel=3, thresh=(0, 255)):
    # Convert to grayscale
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # Take both Sobel x and y gradients
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Calculate the gradient magnitude
    gradmag = np.sqrt(sobelx**2 + sobely**2)
    # Rescale to 8 bit
    scale_factor = np.max(gradmag)/255 
    gradmag = (gradmag/scale_factor).astype(np.uint8) 
    # Create a binary image of ones where threshold is met, zeros otherwise
    binary_output = np.zeros_like(gradmag)
    binary_output[(gradmag >= thresh[0]) & (gradmag <= thresh[1])] = 1

    # Return the binary image
    return binary_output

# Define a function to threshold an image for a given range and Sobel kernel
def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)):
    # Grayscale
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # Calculate the x and y gradients
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # Take the absolute value of the gradient direction, 
    # apply a threshold, and create a binary image result
    absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx))
    binary_output =  np.zeros_like(absgraddir)
    binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1

    # Return the binary image
    return binary_output

def color_threshold(image, sthresh=(0,255), vthresh=(0,255)):
    
    hls = cv2.cvtColor(image, cv2.COLOR_BGR2HLS)
    s_channel = hls[:,:,2]
    s_binary = np.zeros_like(s_channel)
    s_binary[ (s_channel >= sthresh[0]) & (s_channel <= sthresh[1]) ] = 1
    
    hsv = cv2.cvtColor(image, cv2.COLOR_RGB2HSV)
    v_channel = hsv[:,:,2]
    v_binary = np.zeros_like(v_channel)
    v_binary[ (v_channel >= vthresh[0]) & (v_channel <= vthresh[1]) ] = 1
    
    output = np.zeros_like(s_channel)
    output[(s_binary == 1) & (v_binary == 1)] = 1
    
    return output

Now we obtain the preprocessed binary image a combination of GRAD_X AND GRAD_Y and the result we make an OR with Color Threshold.

In [7]:
def detect_line_borders(img):
    preprocessImage = np.zeros_like(img[:,:,0])
    gradx = abs_sobel_thresh(img, orient = 'x', thresh=(12,255))  # 12
    grady = abs_sobel_thresh(img, orient = 'y', thresh=(25,255))  # 25
    mag = mag_thresh(img, thresh=(10,255))
    dirth = dir_threshold(img, thresh=(0.85, 1.15))
    c_binary = color_threshold(img, sthresh=(105,255), vthresh= (205,255))
    
    preprocessImage[(c_binary == 1) ] = 255
    
    preprocessImage[(c_binary == 1) |((gradx == 1)&(grady ==1)) | ((dirth ==1)&(mag == 1))] = 255
    
    return preprocessImage

Test of Step 3

We test the function above with the provided test images undistorted.

In [8]:
undist_images = glob.glob('./output_images/undist*.jpg')

for idx, fname in enumerate(undist_images):
    img = cv2.imread(fname)
    img_size = (img.shape[1], img.shape[0])
    
    preprocessImage = detect_line_borders(img)

    write_name = './output_images/binary' + str(idx+1) + '.jpg'
    cv2.imwrite(write_name, preprocessImage)
    
    plt.imshow(preprocessImage, cmap='gray')
    plt.show()

Step 4. Perspective Transform

In [9]:
src = np.float32([
#    [577,460],
#    [710,460],
#    [260,684], 
#    [1070,684]])
   
    [593,450],
    [691,450],
    [260,684], 
    [1070,684]])
#274,670   1047, 670

dst = np.float32([
    [320,0],
    [960,0],
    [320,720],
    [960,720]])

M = cv2.getPerspectiveTransform(src,dst)
Minv = cv2.getPerspectiveTransform(dst,src)

def perspective_transform_warp(img):
    img_size = (img.shape[1], img.shape[0])
    return cv2.warpPerspective(img,M,img_size)

def perspective_transform_unwarp(img):
    img_size = (img.shape[1], img.shape[0])
    return cv2.warpPerspective(img,Minv,img_size)

We test the function above with the test images previously processed (binaries)

In [10]:
binary_images = glob.glob('./output_images/binary*.jpg')
undist_warped = glob.glob('./output_images/undist*.jpg')
print("Warped original undistorted color images are shown just for reference")
for idx, fname in enumerate(zip(binary_images,undist_warped)):
    binary = cv2.imread(fname[0])
    binary = binary[:,:,0]
    
    undist = image = mpimg.imread(fname[1])

    binary_warped = perspective_transform_warp(binary)
    undist_warped = perspective_transform_warp(undist)
    
    f, (ax1,ax2) = plt.subplots(1,2,figsize=(24,9))
    f.tight_layout()
    ax1.imshow(undist_warped)
    ax2.imshow(binary_warped,cmap='gray')
    plt.subplots_adjust(left=0.,right=1,top=0.9,bottom=0.)
    plt.show()
    write_name = './output_images/warped' + str(idx+1) + '.jpg'
    cv2.imwrite(write_name, binary_warped)

Warped original undistorted color images are shown just for reference

Step 5-6. Fit Lines

In [14]:
import tracker
import line

warped_images = glob.glob('./output_images/warped*.jpg')
undist_images = glob.glob('./output_images/undist*.jpg')

for idx, fname in enumerate(zip(warped_images, undist_images)):
    warped = cv2.imread(fname[0])
    warped = warped[:,:,0]
    undist = cv2.imread(fname[1])
    
    # Set up the overall class to do all the tracking
    
    window_width = 40
    window_height = 80
    margin = 100    
    ym = 40/720
    xm = 3.7/700
    smooth_factor = 15

    
    curve_centers = tracker.Tracker(window_width, window_height, margin, ym, xm, smooth_factor)
    
    window_centroids = curve_centers.find_window_centroids(warped)
    
    left_line_track = line.Line(warped.shape, window_height, window_width)
    right_line_track = line.Line(warped.shape, window_height, window_width)
    
    # Points to draw all the left and right windows
    l_points = np.zeros_like(warped)
    r_points = np.zeros_like(warped)
    
    
    left_line_track.update_centroids(window_centroids[:,0])
    right_line_track.update_centroids(window_centroids[:,1])
        
    
    yvals = range(0, warped.shape[0])
    
    #res_yvals = np.arange(warped.shape[0]-(window_height/2),0,-window_height)
    
    # Fit a second order polynomial to pixel positions in each fake lane line
    
    left_fitx, _ = left_line_track.line_fit()
    right_fitx, _ = right_line_track.line_fit()
    
    y_eval = np.max(yvals)
    
    left_lane = left_line_track.get_lane_for_drawing()
    right_lane = right_line_track.get_lane_for_drawing()
    
    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, yvals]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, yvals])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image 
    road = np.zeros_like(undist)
    cv2.fillPoly(road, [left_lane], color=[255,0,0])
    cv2.fillPoly(road, [right_lane], color=[0,0,255])
    cv2.fillPoly(road, np.int_([pts]), (0,255, 0))
    
    road_warped = cv2.warpPerspective(road, Minv, img_size, flags=cv2.INTER_LINEAR)
    result = cv2.addWeighted(undist ,1.0, road_warped, 0.5, 0.0)
    
    left_curverad = left_line_track.get_line_curvature()
    right_curverad = right_line_track.get_line_curvature()
    
    curverad = (left_curverad + right_curverad) / 2.0
    
    print('Curvature radius is ' + str(abs(round(curverad,3))) + 'm')
    
    #calculate the offset of the car on the road
    camera_center = (left_fitx[-1] + right_fitx[1])/2
    center_diff = (camera_center-warped.shape[1]/2)* xm
    side_pos = 'left'
    if center_diff <=0:
        side_pos = 'right'
    
    print('Vehicle is '+ str(abs(round(center_diff,3))) + 'm ' + side_pos + 'off center')
    
    write_name = './output_images/result' + str(idx+1) + '.jpg'
    cv2.imwrite(write_name, result)
    
    # Plot up the sliding windows and calculated lanes
    mark_size = 3
    plt.imshow(result)
    plt.show()

    

Curvature radius is 1543.12m
Vehicle is 0.558m leftoff center

Curvature radius is 631.105m
Vehicle is 0.33m rightoff center

Curvature radius is 956.175m
Vehicle is 0.595m leftoff center

Curvature radius is 614.405m
Vehicle is 0.682m leftoff center

Curvature radius is 935.229m
Vehicle is 0.436m leftoff center

Curvature radius is 1008.314m
Vehicle is 0.666m leftoff center

Video Pipeline

In [15]:
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
In [16]:
import tracker
import line

window_width = 30
window_height = 80
margin = 50    
ym = 40/720
xm = 3.7/700
smooth_factor_centroids = 10
smooth_factor_polynoms = 5
img_shape = (720,1280,3)

curverad = 0

#Instantiate tracker Class object that will process all the frames in the sequence
pipeline_tracker = tracker.Tracker(window_width, window_height, margin, ym, xm, smooth_factor_centroids)
pipeline_left_lane = line.Line(img_shape, window_height, window_width, ym, xm, smooth_factor_polynoms)
pipeline_right_lane = line.Line(img_shape, window_height, window_width, ym, xm, smooth_factor_polynoms)

# We load the distorsion pickle file data obtained in section 'Step 1' above
dist_pickle = pickle.load( open( "camera_cal/calibration_pickle.p", "rb"))
mtx = dist_pickle["mtx"]
dist = dist_pickle["dist"]

def process_frame(image):

    img_size = (image.shape[1], image.shape[0])

    # STEP 1. UNDISTORT
    ###################
    undist_img = cv2.undistort(image, mtx, dist, None, mtx)
    
    # STEP 2. LANE LINES BORDERS DETECTION (BINARY)
    ###############################################
    binary_img = detect_line_borders(undist_img)
    
    # STEP 3. PERSPECTIVE TRANSFORM
    ###################################################
    warped_img = perspective_transform_warp(binary_img)
    
    # STEPS 4,5,6. DETECT AND FIT LINES
    ###################################################
    yvals = range(0, warped_img.shape[0])
    
    window_centroids = pipeline_tracker.find_window_centroids(warped_img)
    pipeline_left_lane.update_centroids(window_centroids[:,0])
    pipeline_right_lane.update_centroids(window_centroids[:,1])
    
    left_fitx, _ = pipeline_left_lane.line_fit()
    right_fitx, _ = pipeline_right_lane.line_fit()
    
    left_lane_polygon = pipeline_left_lane.get_lane_for_drawing()
    right_lane_polygon = pipeline_right_lane.get_lane_for_drawing()
    
    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, yvals]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, yvals])))])
    pts = np.hstack((pts_left, pts_right))
    
    left_curverad = pipeline_left_lane.get_line_curvature()
    right_curverad = pipeline_right_lane.get_line_curvature()
    
    curverad = (left_curverad + right_curverad) / 2.0
    
    #calculate the offset of the car on the road
    camera_center = (left_fitx[-1] + right_fitx[1])/2
    center_diff = (camera_center-warped_img.shape[1]/2)* xm
    side_pos = 'left'
    if center_diff <=0:
        side_pos = 'right'
    
    # Draw the lane onto the warped blank image 
    road = np.zeros_like(image)
    cv2.fillPoly(road, [left_lane_polygon], color=[255,0,0])
    cv2.fillPoly(road, [right_lane_polygon], color=[0,0,255])
    cv2.fillPoly(road, np.int_([pts]), (0,255, 0))

    road_warped = cv2.warpPerspective(road, Minv, img_size, flags=cv2.INTER_LINEAR)

    result = cv2.addWeighted(undist_img ,1.0, road_warped, 0.5, 0.0)
    
    # Draw the text showing curvature and offset
    cv2.putText(result, 'Radius of Curvature = '+str(round(curverad,3))+'m ',(50,50)
                ,cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,255),2)
    cv2.putText(result, 'Vehicle is ' + str(abs(round(center_diff, 3))) +'m '+side_pos+' of center',
                (50,100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255,255,0),2)
    
    
    
    return result

Try the pipeline with the provided video 'project_video.mp4'

In [ ]:
output_file = 'output_video.mp4'
clip1 = VideoFileClip("project_video.mp4")

output_clip = clip1.fl_image(process_frame)
%time output_clip.write_videofile(output_file, audio=False)